/*
 *  findBallOrientation.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2008-02-25.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "displayOpenCV.h"
#include "findBallOrientation.h"
#include "findBallState.h"
#include "imageCropper.h"
#include "maskGenerate.h"
#include "openCVincludes.h"

#define MARKER_DATA "bandpMarkerData.yml"

extern CvMat *convMat; //This was declared global because I couldn't figure out any other way to kill the memory leak.

extern double calibration;
extern float decimate;
extern double ballDia;
extern int viewSize;
extern SystemDims table;

int identifyOrientation( CvSeq * );
void rotateMarker( IplImage**, CvMat *, CvMat *, CvMat *, double, int );

void orientationFinder(/*Input variables*/  MaskObject *shapes, ImageSet images, CvMat *x_hatOrientation, CvMat *y_measrd, int numMarkers, CvSeq *markersFound, ImageAnalysisParams params,
        /*Output variables*/ int *orientationLocks, CvMat **orientation_measrd ) {
    
//void orientationFinder(, , int numMarkers, double markerSize) {
    
    int thumbHole=identifyOrientation( markersFound ); //This function only works with bowling balls. Need to generalize it.
    
    //Create rotation matrix
    //WE HAVE A PROBLEM WITH SKEW. SOMEHOW, WE HAVE TO MAKE SURE THAT THE COORDINATES ARE ALWAYS ORTHONORMAL
    CvMat *Rot=cvCreateMat( 3, 3, CV_32F );
    
    double xi, yi, zi, xj, yj, zj, xk, yk, zk;
    xi=cvmGet( x_hatOrientation, 0, 0 );
    yi=cvmGet( x_hatOrientation, 1, 0 );
    zi=cvmGet( x_hatOrientation, 2, 0 );
    xk=cvmGet( x_hatOrientation, 3, 0 );
    yk=cvmGet( x_hatOrientation, 4, 0 );
    zk=cvmGet( x_hatOrientation, 5, 0 );
//    for( int j = 0; j < *numMarkersFound; j++ ) {
//        CvPoint pt0 = *CV_GET_SEQ_ELEM( CvPoint, *markersFound, j );
//        printf("pt0= %d, %d\n", pt0.x, pt0.y);
//    }
    
    CvPoint ptThumb = *CV_GET_SEQ_ELEM( CvPoint, markersFound, thumbHole );
    CvPoint ptFinger1 = *CV_GET_SEQ_ELEM( CvPoint, markersFound, (thumbHole+1)%3 );
    CvPoint ptFinger2 = *CV_GET_SEQ_ELEM( CvPoint, markersFound, (thumbHole+2)%3 );
    
    double xtmp, ytmp, ztmp, normtmp;
    normtmp=table.ballDia/2*calibration;
    xtmp=(ptThumb.x-cvmGet(y_measrd, 0, 0));
    ytmp=(ptThumb.y-cvmGet(y_measrd, 1, 0));
    ztmp=-sqrt(pow(normtmp, 2)-pow(xtmp, 2)-pow(ytmp, 2));
//    double normtmp=sqrt(pow(xtmp, 2)+pow(ytmp, 2)+pow(ztmp, 2));
    xk=xtmp/normtmp;
    yk=ytmp/normtmp;
    zk=ztmp/normtmp;
//    printf("thumbHole= %d\n", thumbHole);
//    printf("y_measrd = %f, %f\n", cvmGet(y_measrd, 0, 0), cvmGet(y_measrd, 1, 0));
    
    
//    printf("ptThumb = %d, %d\n", ptThumb.x, ptThumb.y);
//    printf("ptFinger1 = %d, %d\n", ptFinger1.x, ptFinger1.y);
//    printf("ptFinger2 = %d, %d\n", ptFinger2.x, ptFinger2.y);
//    printf("THUMB: xtmp = %f, ytmp = %f, ztmp = %f, normtmp = %f\n", xtmp, ytmp, ztmp, normtmp );
    
    xtmp=((ptFinger1.x+ptFinger2.x)/2.0-cvmGet(y_measrd, 0, 0));
    ytmp=((ptFinger1.y+ptFinger2.y)/2.0-cvmGet(y_measrd, 1, 0));
    ztmp=-sqrt(pow(normtmp, 2)-pow(xtmp, 2)-pow(ytmp, 2));
    
    xj=ytmp*zk-ztmp*yk;
    yj=ztmp*xk-xtmp*zk;
    zj=xtmp*yk-ytmp*xk;
    normtmp=sqrt(xj*xj+yj*yj+zj*zj);
    
    xj=xj/normtmp;
    yj=yj/normtmp;
    zj=zj/normtmp;
    
    xi=yk*zj-zk*yj;
    yi=zk*xj-xk*zj;
    zi=xk*yj-yk*xj;
    
    //Rot is a matrix whose vector norms=1
    cvmSet( Rot, 0, 0, xi );
    cvmSet( Rot, 0, 1, xj );
    cvmSet( Rot, 0, 2, -xk  );
    cvmSet( Rot, 1, 0, yi );
    cvmSet( Rot, 1, 1, yj );
    cvmSet( Rot, 1, 2, -yk  );
    cvmSet( Rot, 2, 0, zi );
    cvmSet( Rot, 2, 1, zj );
    cvmSet( Rot, 2, 2, -zk  );
    
//    printCvArr(Rot);
    
#if defined DEBUG && (DEBUG > 2)
    CvMat *RotTEST=cvCreateMat(3, 3, CV_32F);
    cvMulTransposed( Rot, RotTEST, 0);
    printCvArr(Rot);
    printCvArr(RotTEST);
    
    cvReleaseMat(&RotTEST);
#endif
    
//    double scaleAxes=100;
//    cvLine( images.edgeImage, cvPoint(cvRound(cvmGet(y_measrd, 0, 0)-images.cropRect.x), cvRound(cvmGet(y_measrd, 1, 0)-images.cropRect.y)), cvPoint((ptFinger1.x+ptFinger2.x)/2-images.cropRect.x, cvRound((ptFinger1.y+ptFinger2.y)/2-images.cropRect.y)), CV_RGB( 255, rand()&255, rand()&255 ), 2 );
//    cvLine( images.edgeImage, cvPoint(cvRound(cvmGet(y_measrd, 0, 0)-images.cropRect.x), cvRound(cvmGet(y_measrd, 1, 0)-images.cropRect.y)), cvPoint(cvRound(cvmGet(y_measrd, 0, 0)-images.cropRect.x+xk*scaleAxes), cvRound(cvmGet(y_measrd, 1, 0)-images.cropRect.y+yk*scaleAxes)), CV_RGB( 255, rand()&255, rand()&255 ), 2 );
//    cvShowImage("Parameters", images.edgeImage);
    
    
//    IplImage *cropImage, *grayImage, *bwImage ;
    IplImage *edgeImage;
    CvMat *convMatShape;
    CvMat *dft_mIm=cvCreateMatHeader( 2, 1, CV_32F );
    int markerSize;
    double convMin=0, convMax=0;
    CvRect cropRect;
    
#if defined DEBUG && (DEBUG > 2)
    printf("Error 110...\n");
#endif
    int orientationLocksOld=*orientationLocks;
    *orientationLocks=0;
    
    for (int i=0; i<numMarkers; i++){
        int j=(i+thumbHole) % 3;
        
        if (orientationLocksOld!=numMarkers){
            CvPoint pt0 = *CV_GET_SEQ_ELEM( CvPoint, markersFound, j );
            shapes[i].pixCoords[0] = pt0.x-images.cropRect.x;
            shapes[i].pixCoords[1] = pt0.y-images.cropRect.y;
//            printCvArr(shapes[i].pixCoordsMat);
        }
        else{
            cvMatMul(Rot, shapes[i].spatialCoordsMat, shapes[i].spatialCoordsMat);
            shapes[i].pixCoords[0] = shapes[i].spatialCoords[0]*calibration;
            shapes[i].pixCoords[1] = shapes[i].spatialCoords[1]*calibration;
                        CvPoint pt0 = *CV_GET_SEQ_ELEM( CvPoint, markersFound, j );
            shapes[i].pixCoords[0] = pt0.x-images.cropRect.x;
            shapes[i].pixCoords[1] = pt0.y-images.cropRect.y;
//            printCvArr(shapes[i].pixCoordsMat);
        }
        
        
        markerSize=shapes[i].diaPix;
        //viewSize=markerSize*1;
        
        imageCropper( &cropRect, shapes[i].pixCoordsMat, cvRect( 0, 0, images.grayImage->width, images.grayImage->height ), markerSize, 0, 0);
        cvSetImageROI( images.edgeImage, cropRect );
        
#if defined DEBUG &&(DEBUG > 2)
        printf("Error 112...\n");
#endif
        
//        cropImage = cvCreateImage(cvGetSize(images.grayImage), images.edgeImage->depth, images.edgeImage->nChannels);
//        grayImage = cvCreateImage(cvGetSize(cropImage), cropImage->depth, 1);
//        bwImage = cvCreateImage(cvGetSize(cropImage), cropImage->depth, 1);
        edgeImage = cvCreateImage(cvGetSize(images.edgeImage), images.edgeImage->depth, images.edgeImage->nChannels);
        cvZero(edgeImage);
        cvCopy( images.edgeImage, edgeImage );
        
        
        
//        cvCopy( images.grayImage, cropImage );
//
//        if (cropImage->nChannels!=1){
//            cvSplit( cropImage, grayImage, NULL, NULL, NULL );
////            //	cvCvtColor(cropImage, grayImage, CV_BGR2GRAY);	//Can't figure out how to tell if it's a RGB or
////            //grayscale because my camera returns three
////            //channels, even though it's grayscale.
//        }
//        else{
//            cvCopy(cropImage, grayImage);
//        }
        
//        cvCopy( grayImage, bwImage );
//        cvCanny( bwImage, edgeImage, params.cannyThreshLow, params.cannyThreshHigh, 3 ); // Performs the edge extraction on the image
        
//    cvShowImage("Parameters", images.edgeImage);
//    cvWaitKey(0);
//        cvSetImageROI( images.grayImage, cropRect );
        
#if defined DEBUG && (DEBUG > 2)
        printf("Error 113...\n");
#endif
        IplImage* markerImage = cvCreateImage( cvSize(markerSize, markerSize),  IPL_DEPTH_8U, 1 );
        rotateMarker( &markerImage, Rot, shapes[i].rotationMat , shapes[i].points, calibration, markerSize);
        int dft_rows = cvGetOptimalDFTSize( edgeImage->height + markerImage->height - 1 );	//This code could be done offline, but there's no reason to because it's so fast.
        int dft_cols = cvGetOptimalDFTSize( edgeImage->width + markerImage->width - 1 );		//This code could be done offline, but there's no reason to because it's so fast.
        
#if defined DEBUG && (DEBUG > 2)
        printf("Error 115...\n");
#endif
        CvPoint ballPoint;
        maskDFT( dft_rows, dft_cols, 1 /*always redo the DFFT*/, markerImage, &dft_mIm );
        conv2D( /*Input variables*/     edgeImage, markerImage, dft_mIm, dft_rows, dft_cols,
                /*Output variables*/	&convMatShape);
        cvMinMaxLoc( convMatShape, &convMin, &convMax, NULL, &ballPoint);

#if defined DEBUG && (DEBUG > 2)
        printf("Error 118...\n");
#endif
        
//          cvShowImage("Ball and Plate", images.edgeImage);
//        CvMat tmp;
//        cvGetSubRect(images.edgeImage, &tmp, cvRect(0, 0, markerImage->width, markerImage->height ));
//        cvOr(&tmp, markerImage, &tmp);
//        cvShowImage( "Parameters", images.edgeImage);
//        cvShowImage( "Ball and Plate", markerImage);
////        cvWaitKey(0);
//        cvShowImage( "Ball and Plate", &tmp);
        
//        imagesc( "Parameters", convMatShape );
//        cvWaitKey(0);
        
        
        //cvShowImage( "Parameters", markerImage );
//        cvShowImage( "Ball and Plate", images.edgeImage );
        
        
        
        if ( convMax > 1.8e06){ //convMax SHOULD BE GENERICALLY DEFINED SOMEHOW.
            (*orientationLocks)++;
            shapes[i].found=true;
            
            // Find the center of the marker to sub-pixel resolution
            centerOfMoments(edgeImage, convMatShape, ballPoint, cvRect(images.cropRect.x+cropRect.x, images.cropRect.y+cropRect.y, 0, 0 ), &shapes[i].pixCoordsMat);
//        printf("pt = %d, %d, pt pixel, %d, %d, pt subpixel = %f, %f\n", pt0.x, pt0.y, ballPoint.x, ballPoint.y, shapes[i].pixCoords[0], shapes[i].pixCoords[1]);
        }
        else{
            printf("Orientation convolution threshold might be set too high. Max conv. value: %4.3e.\n", convMax);
            /*missingCount=*missingCount+1*/;
        }
        
        //Loop garbage collection
        cvReleaseImage( &markerImage);
        cvReleaseImage( &edgeImage);
        cvReleaseMat( &convMatShape );
        cvReleaseMat( &convMat );       
    }

    //Calculate and output measured coordinate axes.
    double xkOrien=shapes[0].pixCoords[0];
    double ykOrien=shapes[0].pixCoords[1];
    double xiOrien=(shapes[1].pixCoords[0]+shapes[2].pixCoords[0])/2-cvmGet(y_measrd, 0, 0);
    double yiOrien=(shapes[1].pixCoords[1]+shapes[2].pixCoords[1])/2-cvmGet(y_measrd, 1, 0);
    
    normtmp=table.ballDia/2*calibration;
    xtmp=(xkOrien-cvmGet(y_measrd, 0, 0));
    ytmp=(ykOrien-cvmGet(y_measrd, 1, 0));
    ztmp=-sqrt(pow(normtmp, 2)-pow(xtmp, 2)-pow(ytmp, 2));
    
    xk=xtmp/normtmp;
    yk=ytmp/normtmp;
    zk=ztmp/normtmp;
//    printf("xtmp = %f, ytmp = %f, ztmp = %f, normtmp = %f\n", xtmp, ytmp, ztmp, normtmp );
    
    ztmp=-sqrt(pow(normtmp, 2)-pow(xiOrien, 2)-pow(yiOrien, 2));
//    printf("xiOrien = %f, yxiOrien = %f, ztmp = %f, normtmp = %f\n", xiOrien, yiOrien, ztmp, normtmp );

    xj=yiOrien*zk-ztmp*yk;
    yj=ztmp*xk-xiOrien*zk;
    zj=xiOrien*yk-yiOrien*xk;
    normtmp=sqrt(xj*xj+yj*yj+zj*zj);
//    printf("xj = %f, yj = %f, zj = %f, normtmp = %f\n", xj, yj, zj, normtmp );
    
    xj=xj/normtmp;
    yj=yj/normtmp;
    zj=zj/normtmp;
    
    xi=yk*zj-zk*yj;
    yi=zk*xj-xk*zj;
    zi=xk*yj-yk*xj;

    

        
        cvmSet( *orientation_measrd, 0, 0, xi );
        cvmSet( *orientation_measrd, 1, 0, yi);
//        cvmSet( *orientation_measrd, 2, 0, zi);
        cvmSet( *orientation_measrd, 2, 0, xk );
        cvmSet( *orientation_measrd, 3, 0, yk);
//        cvmSet( *orientation_measrd, 5, 0, zk );
        
             //   printCvArr(*orientation_measrd);

    
    //Garbage collection
    cvReleaseMat( &Rot );
    cvReleaseMat( &dft_mIm);
    
    cvResetImageROI( images.edgeImage );    
    
    return;
}

//=============================================================================
//=============================================================================
//=============================================================================
//=============================================================================


void rotateMarker(IplImage **markerImage, CvMat *Rot, CvMat *preRotation, CvMat *marker, double calibration, int markerSize){
    
    //markerSize MUST BE EVEN. DO THE CHECK SOMEWHERE ELSE.
    
    CvMat *rotatedMarker=cvCreateMat( marker->rows, 3, CV_32F );
    double center[2]; //MEMORY LEAK? IS FREE() THE RIGHT WAY TO DO THIS, OR DOES IT TAKE CARE OF THINGS ON ITS OWN?
    
    CvMat tmp1, *tmp2, *tmp33;
    tmp2=cvCreateMat( 3, 1, CV_32F );
    tmp33=cvCreateMat( 3, 3, CV_32F );
    cvMatMul(Rot, preRotation, tmp33);
//    printCvArr(tmp33);
    
    //Rotate the image by applying the rotation matrix to each point.
    for (int i=0; i<rotatedMarker->rows; i++){
        
        
        cvGetRow(marker, &tmp1, i);
        cvGEMM(tmp33, &tmp1, 1, 0, 0, tmp2, CV_GEMM_B_T);
        
        cvmSet( rotatedMarker, i, 0, cvmGet( tmp2, 0, 0) );
        cvmSet( rotatedMarker, i, 1, cvmGet( tmp2, 1, 0) );
        cvmSet( rotatedMarker, i, 2, cvmGet( tmp2, 2, 0) );
    }
//    printCvArr(rotatedMarker);
    //Find the center of the rotated coordinates
    //***Should probably use a CvBox here and do a cvBoundingRect, but don't know how yet.***///
    for (int i=0; i<2; i++){
        CvMat tmp1;
        cvGetCol(rotatedMarker, &tmp1, i);
        center[i]=(double)(cvAvg( &tmp1 )).val[0];
    }
    
//        printf("center 1 and 2 = %f, %f\n", center[0], center[1]);
    
    // Generate image
    cvZero(*markerImage);
    for (int i=0; i < rotatedMarker->rows; i++){
//        printf("Error %f, %f\n", (cvmGet(rotatedMarker, i, 0)-center[0]), (cvmGet(rotatedMarker, i, 1)-center[1]));
//        printf("Error %d -> %d, %d\n", markerSize, markerSize/2+cvRound((cvmGet(rotatedMarker, i, 0)-center[0])*calibration), markerSize/2+cvRound((cvmGet(rotatedMarker, i, 1)-center[1])*calibration));
        cvSetReal2D( *markerImage, markerSize/2+cvRound((cvmGet(rotatedMarker, i, 1)-center[1])*calibration), markerSize/2+cvRound((cvmGet(rotatedMarker, i, 0)-center[0])*calibration), 255 );
        
    }
    
    //Garbage collection
    cvReleaseMat( &rotatedMarker );
    cvReleaseMat( &tmp2 );
    cvReleaseMat( &tmp33 );
    
    return;
}

void createCircularMarker( double ballDia, double holeDia, char *markerName ) {
    double phi=asin(holeDia/(2*ballDia));
    CvMat* marker=cvCreateMat( (int)floor(3.14/.1+1)*2, 3, CV_32F );
    
    //Make one half of the circle
    int j=0;
    for (double i=0; i<3.14; i=i+0.1){
        cvmSet( marker, j, 0, ballDia*cos(i)*sin(phi) );
        cvmSet( marker, j, 1, ballDia*sin(i)*sin(phi) );
        cvmSet( marker, j, 2, ballDia*cos(phi) );
        j=j+1;
    }
    
    //Make the other half
    for (int i=0; i<j; i++) {
        cvmSet( marker, i+j, 0, cvmGet( marker, j-i-1, 0) );
        cvmSet( marker, i+j, 1, -cvmGet( marker, j-i-1, 1) );
        cvmSet( marker, i+j, 2, cvmGet( marker, j-i-1, 2) );
    }
    CvFileStorage* markerData = cvOpenFileStorage( MARKER_DATA, 0, CV_STORAGE_WRITE);
    cvWrite( markerData, markerName, marker );
    
    //Garbage collection
    cvReleaseFileStorage( &markerData );
    cvReleaseMat( &marker );
    
    return;
    
}

//=============================================================================
//=============================================================================
//=============================================================================
//=============================================================================


void loadMarkers( MaskObject **shapes, int *numMarkers ) {
    CvFileStorage* markerData = cvOpenFileStorage( MARKER_DATA, 0, CV_STORAGE_READ);
    *numMarkers=cvReadIntByName( markerData, NULL, "numMarkers", 3 );
    *shapes=(MaskObject*)calloc(*numMarkers, sizeof(MaskObject) );
    char marker[10];
    char markerRot[16];
    
    CvMat tmp;
    double xMin, xMax, yMin, yMax;
        
    //Create the shape objects
    for (int i=0; i<*numMarkers; i++){
        
        //Create the matrices
        (*shapes)[i].pixCoordsMat=cvCreateMatHeader( 2, 1, CV_32F );
        (*shapes)[i].spatialCoordsMat=cvCreateMatHeader( 3, 1, CV_32F );
        
        //Assigns the pixCoords and pixCoordsMat to the same data set.
        //This is useful in the program because sometimes it's far easier
        //to access one or the other. Likewise for spatialCoordsMat
        cvInitMatHeader( (*shapes)[i].pixCoordsMat, 2, 1, CV_32F, (*shapes)[i].pixCoords );
        cvInitMatHeader( (*shapes)[i].spatialCoordsMat, 3, 1, CV_32F, (*shapes)[i].spatialCoords );
        
        //Fill the points
        sprintf(marker, "marker%d", i+1);
        (*shapes)[i].points=(CvMat*)cvReadByName( markerData, NULL, marker/*, 2, 3, 4, ...*/ );
        
        //Calculate object size
        cvGetCol((*shapes)[i].points, &tmp, 0);
        cvMinMaxLoc( &tmp, &xMin, &xMax, NULL, NULL);
        
        cvGetCol((*shapes)[i].points, &tmp, 1);
        cvMinMaxLoc( &tmp, &yMin, &yMax, NULL, NULL);
        
        if(fabs(xMax-xMin) >= fabs(yMax-yMin))
            (*shapes)[i].dia=fabs(xMax-xMin);
        else
            (*shapes)[i].dia=fabs(yMax-yMin);
        
        (*shapes)[i].diaPix=cvRound((*shapes)[i].dia*calibration)+2;
        if (((*shapes)[i].diaPix % 2) == 0)
            (*shapes)[i].diaPix=(*shapes)[i].diaPix+1;
        
        (*shapes)[i].rotationMat=cvCreateMat(3, 3, CV_32F);
        sprintf(markerRot, "marker%dRotMat", i+1);
        (*shapes)[i].rotationMat=(CvMat*)cvReadByName( markerData, NULL, markerRot);        
    }
    
    // Garbage collection
    cvReleaseFileStorage( &markerData );
    return;
}






void markerLocalizer(CvSeq **output, ImageSet images, MaskObject ball, MaskObject *shapes, double numMarkers, int *numMarkersFound, ImageAnalysisParams params ) {
    
//    CvRect cropRect;
//    imageCropper( &cropRect, ball.pixCoordsMat, cvRect( 0, 0, camFrame->width, camFrame->height ), ball.image->width, 0, 0);
//    cvSetImageROI( camFrame, cropRect );
    
//    IplImage *cropImage, *grayImage, *bwImage ;
    IplImage *edgeImage;
    
#if defined DEBUG && (DEBUG > 3)
    printf("Error 10\n");
#endif
    
//    cropImage = cvCreateImage(cvGetSize(camFrame), camFrame->depth, camFrame->nChannels);
//    grayImage = cvCreateImage(cvGetSize(cropImage), cropImage->depth, 1);
//    bwImage = cvCreateImage(cvGetSize(cropImage), cropImage->depth, 1);
    edgeImage = cvCreateImage(cvGetSize(images.edgeImage), images.edgeImage->depth, 1);
    cvZero(edgeImage);
    cvCopy( images.edgeImage, edgeImage, ball.image );
    
//    cvZero(cropImage);
//    cvCopy( camFrame, cropImage, ball.image );
    
#if defined DEBUG && (DEBUG > 3)
    printf("Error 40\n");
#endif
    
//    if (cropImage->nChannels!=1){
//        cvSplit( cropImage, grayImage, NULL, NULL, NULL );
//        //	cvCvtColor(cropImage, grayImage, CV_BGR2GRAY);	//Can't figure out how to tell if it's a RGB or
//        //grayscale because my camera returns three
//        //channels, even though it's grayscale.
//    }
//    else{
//        cvCopy(cropImage, grayImage);
//    }
//
//    cvCopy( grayImage, bwImage );
//    cvCanny( bwImage, edgeImage, params.cannyThreshLow, params.cannyThreshHigh, 3 ); // Performs the edge extraction on the image
    
    
    CvMemStorage *storage = NULL, *storage2 = NULL;
    int header_size;
    CvSeq *contour = NULL;
    cvClearSeq( *output );
    
    CvBox2D32f* ellipseBox;
    CvPoint* PointArray;
    CvPoint2D32f* PointArray2D32f;
    
// initialize members
    header_size = sizeof(CvContour);
    storage = cvCreateMemStorage(0);
    
    int minimumFeatureSize=(int) 1e06;
    for (int i=1; i<numMarkers; i++) {
        minimumFeatureSize = std::min( minimumFeatureSize, shapes[i].diaPix);
    }
    int maximumFeatureSize=0;
    for (int i=1; i<numMarkers; i++) {
        maximumFeatureSize = std::max( maximumFeatureSize, shapes[i].diaPix);
    }
    
    minimumFeatureSize=(int)(minimumFeatureSize-5); //Enlarge the possibilities so the thresholding isn't too tight.
    maximumFeatureSize=(int)(maximumFeatureSize+5);
    
#if defined DEBUG && (DEBUG > 1)
    printf("Min/Max = %d, %d\n", minimumFeatureSize, maximumFeatureSize );
#endif
    
//    printf("edgeImage = %d, %d\n", images.edgeImage->height, images.edgeImage->width);
//    printf("images.edgeImage = %d, %d\n", images.edgeImage->height, images.edgeImage->width);
//            printf("cropRect = %d, %d\n", cropRect.width, cropRect.height);
    
//    cvShowImage( "Parameters" , edgeImage);
//    cvWaitKey(0);
//        cvShowImage( "Parameters" , edgeImage);
//    cvWaitKey(0);
    
    
// find contours at edgeImageOUT after using cvCanny()
    cvFindContours(edgeImage, storage, &contour, header_size, CV_RETR_LIST, CV_CHAIN_APPROX_NONE );
    *numMarkersFound=0;
//    int count=0;
    
//    CvSeq *firstContour=contour;
    
//    for(;firstContour;firstContour = firstContour->h_next)
//        count++;
//    CvMat *tmpPoints= cvCreateMat(count, 2, CV_32F);
//    CvMat *tmpPoints= cvCreateMat(count, 2, CV_32F);
//    cvZero(tmpPoints);
//    count=0;
//    contour=firstContour;
    // This cycle draws all contours and approximate them by ellipses.
    for(;contour;contour = contour->h_next) {
        int pointCount = contour->total; // This is number point in contour
        CvPoint center;
        CvSize axes;
        
        // Number point must be more than or equal to 6 (for cvFitEllipse_32f).
        if( pointCount < 6 )
            continue;
        
        // get bounding rect of this contour
        CvRect rect;
        rect = cvBoundingRect( contour, 0 );
        
        // Maximum and minimum size for features.
        if ( (rect.width > maximumFeatureSize) || (rect.height > maximumFeatureSize) )
            continue;
//            cvSetRemoveByPtr( pointCount, contour );
        else if( (rect.width < minimumFeatureSize) && (rect.height < minimumFeatureSize) )
            continue;
//            cvSetRemoveByPtr( pointCount, contour );
        
        // Alloc memory for contourour point set.
        PointArray = (CvPoint*)malloc( pointCount*sizeof(CvPoint) );
        PointArray2D32f= (CvPoint2D32f*)malloc( pointCount*sizeof(CvPoint2D32f) );
        
        // Alloc memory for ellipse data.
        ellipseBox = (CvBox2D32f*)malloc(sizeof(CvBox2D32f));
        
        // Get contourour point set.
        cvCvtSeqToArray(contour, PointArray, CV_WHOLE_SEQ);
        
        // Convert CvPoint set to CvBox2D32f set.
        for(int i=0; i<pointCount; i++) {
            PointArray2D32f[i].x = (float)PointArray[i].x;
            PointArray2D32f[i].y = (float)PointArray[i].y;
        }
        
        // Fits ellipse to current contour.
        cvFitEllipse( PointArray2D32f, pointCount, ellipseBox );
        
        // Draw current contour.
//        cvDrawContours(edgeImageOUT,contour,CV_RGB(255,255,255),CV_RGB(255,255,255),0,1,8,cvPoint(0,0));
        
        // Convert ellipse data from float to integer representation.
        center.x = cvRound(ellipseBox->center.x);
        center.y = cvRound(ellipseBox->center.y);
        axes.width = cvRound(ellipseBox->size.width*0.5);
        axes.height = cvRound(ellipseBox->size.height*0.5);
        ellipseBox->angle = -ellipseBox->angle;
        
        if ( (2*axes.width > maximumFeatureSize) || (2*axes.height > maximumFeatureSize) );
        // Ellipse bigger than any feature.
        else if( (2*axes.width < minimumFeatureSize) && (2*axes.height < minimumFeatureSize) );
        // Ellipse smaller than any feature.
        else{
            // Draw ellipse.
            cvEllipse(edgeImage, center, axes, ellipseBox->angle, 0, 360,
                    CV_RGB( rand()&255, rand()&255, 255), 2, CV_AA, 0);
            center.x+=images.cropRect.x;
            center.y+=images.cropRect.y;
            cvSeqPush(*output, &center);
            (*numMarkersFound)++;
            
        }
        // Loop garbage collection
        free(PointArray);
        free(PointArray2D32f);
        free(ellipseBox);
    }
       
    for( int i=0; i<*numMarkersFound; i++ ) {
        CvPoint pt0 = *CV_GET_SEQ_ELEM( CvPoint, *output, i );
        double s01;
        CvPoint pt1;
        for (int j=i+1; j < *numMarkersFound; j++){
            pt1 = *CV_GET_SEQ_ELEM( CvPoint, *output, j );
            s01=sqrt(pow(pt1.x-pt0.x, 2)+pow(pt1.y-pt0.y, 2));
            if (s01<7){ //The distance of 7 is completely arbitrary. Need to define this better somewhere else. It should be in function of the size of the rectangle we will use for the convolution.
                cvSeqRemove( *output, j);
                (*numMarkersFound)--;
                j--;
            }
            
        }
    }
    
    
#if defined DEBUG && (DEBUG > 2)
    for( int j = 0; j < *numMarkersFound; j++ ) {
        CvPoint pt0 = *CV_GET_SEQ_ELEM( CvPoint, *output, j );
        printf("FINAL: pt0= %d, %d\n", pt0.x, pt0.y);
    }
//    printCvArr(tmpPoints);
#endif
    
    cvShowImage("Parameters", edgeImage);
    
//    cvWaitKey(0);
    
// Garbage collection
//    cvResetImageROI( camFrame );
//    cvReleaseImage( &cropImage );
//    cvReleaseImage( &grayImage );
//    cvReleaseImage( &bwImage );
    cvReleaseImage( &edgeImage );
//    cvReleaseMat(&tmpPoints);
    cvReleaseMemStorage(&storage);
}


int identifyOrientation( CvSeq *markersFound ){
    int thumbHole;
    CvPoint pt0 = *CV_GET_SEQ_ELEM( CvPoint, markersFound, 0 );
    CvPoint pt1 = *CV_GET_SEQ_ELEM( CvPoint, markersFound, 1 );
    CvPoint pt2 = *CV_GET_SEQ_ELEM( CvPoint, markersFound, 2 );
    
    double s01=sqrt(pow(pt1.x-pt0.x, 2)+pow(pt1.y-pt0.y, 2));
    double s02=sqrt(pow(pt2.x-pt0.x, 2)+pow(pt2.y-pt0.y, 2));
    double s12=sqrt(pow(pt1.x-pt2.x, 2)+pow(pt1.y-pt2.y, 2));
    
    if( std::min(s01, s02) > s12 )
        thumbHole=0;
    else if (s01 > s02)
        thumbHole=1;
    else
        thumbHole=2;
    
    
#if defined DEBUG && (DEBUG > 4)
//    printf("thumbHole= %d\n", thumbHole);
//    printf("s01, s02, s12= %f, %f, %f\n", s01, s02, s12);
#endif
    return thumbHole;
}
